from launch import LaunchDescription
from launch_ros.actions import Node
from moveit_configs_utils import MoveItConfigsBuilder


def generate_launch_description():
    # 加载MoveIt配置
    moveit_config = MoveItConfigsBuilder("chaojikeaidexiaoche", package_name="chaojixiaoche_moveit").to_moveit_configs()
    
    # arm_control action服务器节点
    arm_control_node = Node(
        package='arm_control',
        executable='arm_control_node',
        name='arm_control_server',
        output='screen',
        parameters=[
            moveit_config.robot_description,
            moveit_config.robot_description_semantic,
            moveit_config.robot_description_kinematics,
            moveit_config.joint_limits,
            moveit_config.planning_pipelines,
        ],
    )
    
    # 轨迹发布节点
    trajectory_publisher_node = Node(
        package='arm_control',
        executable='trajectory_publisher_node',
        name='trajectory_publisher',
        output='screen',
        parameters=[
            {'publish_interval_ms': 100},  # 每100ms发布一个轨迹点
            {'point_skip_interval': 2}     # 每2个点发布一个（减少转发点数量）
        ],
    )

    return LaunchDescription([
        arm_control_node,
        trajectory_publisher_node,
    ])

